import cv2 as cv
import numpy as np
import random
import math

center = (320, 240)
# param = [[[156, 180], [43, 255], [46, 255]], [[0, 180], [0, 255], [0, 46]]]
param = [(155, 43, 46), (180, 255, 255), (0, 0, 0), (180, 255, 60)]
param_1 = [(0, 43, 46), (10, 255, 255), (0, 0, 0), (180, 255, 60)]
Left = 1
Straight = 3
Right = 2
Y = 100
Y_UP = 20
Y_DOWN = 120
X = 40
XX = 140


def stackImages(scale, img_array):
	rows = len(img_array)
	cols = len(img_array[0])
	rowsAvailable = isinstance(img_array[0], list)
	width = img_array[0][0].shape[1]
	height = img_array[0][0].shape[0]
	if rowsAvailable:
		for x in range(0, rows):
			for y in range(0, cols):
				if img_array[x][y].shape[:2] == img_array[0][0].shape[:2]:
					img_array[x][y] = cv.resize(img_array[x][y], (0, 0), None, scale, scale)
				else:
					img_array[x][y] = cv.resize(img_array[x][y], (img_array[0][0].shape[1], img_array[0][0].shape[0]),
					                            None, scale, scale)
				if len(img_array[x][y].shape) == 2:
					img_array[x][y] = cv.cvtColor(img_array[x][y], cv.COLOR_GRAY2BGR)
		imageBlank = np.zeros((height, width, 3), np.uint8)
		hor = [imageBlank] * rows
		for x in range(0, rows):
			hor[x] = np.hstack(img_array[x])
		ver = np.vstack(hor)
	else:
		for x in range(0, rows):
			if img_array[x].shape[:2] == img_array[0].shape[:2]:
				img_array[x] = cv.resize(img_array[x], (0, 0), None, scale, scale)
			else:
				img_array[x] = cv.resize(img_array[x], (img_array[0].shape[1], img_array[0].shape[0]), None, scale,
				                         scale)
			if len(img_array[x].shape) == 2:
				img_array[x] = cv.cvtColor(img_array[x], cv.COLOR_GRAY2BGR)
		hor = np.hstack(img_array)
		ver = hor
	return ver


def BubbleSort(arr):
	length = len(arr)
	while length > 0:
		for i in range(length - 1):
			if arr[i][1] > arr[i + 1][1]:
				var = arr[i]
				arr[i] = arr[i + 1]
				arr[i + 1] = var
		length -= 1


def ToolCal(x):
	return 80.465 * (x ** 3) - 65.415 * (x ** 2) + 46.215 * x + 11.646


def Scope(up, down):
	dx = up[0] - down[0]
	if dx == 0:
		return 0
	else:
		dy = up[1] - down[1]
		return dy / dx


def Mutation(row):
	increase_points = []
	decrease_points = []
	half = len(row) // 2
	length = len(row) - 1

	for i in range(half, length):
		if row[i + 1] > row[i]:
			# print("increase_right", i)
			increase_points.append([i, abs(i - half)])
		if row[i] > row[i + 1]:
			# print("decrease_right", i)
			decrease_points.append([i, abs(i - half)])

	for i in range(half, 0, -1):
		if row[i - 1] < row[i]:
			# print("increase_left", i)
			increase_points.append([i, abs(i - half)])
		if row[i] < row[i - 1]:
			# print("decrease_left", i)
			decrease_points.append([i, abs(i - half)])

	if not increase_points or not decrease_points:
		return 0
	else:
		BubbleSort(increase_points)
		BubbleSort(decrease_points)
		if len(increase_points) > 2:
			# print(increase_points, decrease_points)
			pass

		return [increase_points, decrease_points]


class Line:
	direction = 0
	__horizon = 0
	__line = []
	__line_h = []

	__distance = 0
	cross = (0, 0)
	__frame = None
	__r = None
	__scope = 0
	height = 0
	width = 0

	def __init__(self, height, width):
		self.height = height
		self.width = width

	def GetAngle(self):
		return math.tan(self.__scope)

	def Processing(self, img):
		# gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
		# blur = cv.GaussianBlur(gray, (5, 5), 1)
		# canny = cv.Canny(blur, 200, 200)

		hsv = cv.cvtColor(img, cv.COLOR_BGR2HSV)

		red_1 = cv.inRange(hsv, param[0], param[1])
		red_2 = cv.inRange(hsv, param_1[0], param_1[1])
		red = cv.bitwise_or(red_1, red_2)

		kernel = cv.getStructuringElement(cv.MORPH_RECT, (5, 5))
		dilation_red = cv.dilate(red, kernel, iterations=2)
		blur = cv.GaussianBlur(dilation_red, (5, 5), 1)
		# erosion_red = cv.erode(dilation_red, kernel, iterations=1)
		r, thresh = cv.threshold(blur, 0, 255, cv.THRESH_OTSU)
		ret, thresh_r = cv.threshold(dilation_red, 0, 255, cv.THRESH_OTSU)

		# black = cv.inRange(hsv, param[2], param[3])
		# mor = cv.morphologyEx(red, )
		# dilation_black = cv.dilate(black, kernel, iterations=2)
		# erosion_black = cv.erode(dilation_black, kernel, iterations=1)
		# ret, thresh_b = cv.threshold(erosion_black, 0, 255, cv.THRESH_OTSU)

		# row_r = Mutation(thresh_r)
		self.__frame = img
		self.__r = thresh_r
		stack = stackImages(0.6, [[blur, hsv, red], [dilation_red, thresh, thresh_r]])
		cv.imshow('stack', stack)
		# cv.imshow("thresh", thresh)
		return thresh_r

	def FindLine(self):
		for i in range(self.height - 1, 0, -self.height // 8):
			roi = self.__r[i:i + 1, :]
			mutation_points = Mutation(roi[0])
			positions = []

			if mutation_points != 0:
				for j in range(0, min(len(mutation_points[0]), len(mutation_points[1]))):
					positions.append(
						int((mutation_points[0][j][0] + mutation_points[1][j][0]) / 2))
					point = (min(positions), i)
					self.__line.append(point)
		if len(self.__line) != 8:
			# print(self.__line)
			pass

	def FindHosLine(self, left, right):
		if self.__horizon:
			positions = []

			if (self.width - right - left // 2) // 8 == 0:
				left, right = 80, 300
			# print(left, right)
			for i in range(8):
				# rand = random.randint(left//2, self.width-right//2)
				rand = self.width//(i+1) - 20 + random.randint(-10, 10)
				roi = self.__r[:, rand:(rand + 1)]
				# print(roi)
				roi_row = []
				for x in range(len(roi)):
					roi_row.append(roi[x][0])
				# cv.imshow("roi", self.__r[10:480, i:i+200])
				mutation_points = Mutation(roi_row)
				# print(mutation_points)
				if mutation_points != 0:
					for j in range(0, min(len(mutation_points[0]), len(mutation_points[1]))):
						positions.append(
							int((mutation_points[0][j][0] + mutation_points[1][j][0]) / 2))
						point = (i, min(positions))
						# print(self.__line_h)
						self.__line_h.append(point)

	def HasHorizon(self):
		x = []
		for point in self.__line:
			x.append(point[0])

		if len(x) > 3:
			l, r = min(x), max(x)
		else:
			l, r = 0, self.width
		# print(x, "l, r", l, r)
		p = 0

		for i in range(self.height):
			if self.__r[i][(r - l) // 2] > 0:
				cv.circle(self.__frame, ((r - l) // 2, i), 2, (0, 255, 255), -1)
				p += 1
			if p > 5:
				self.__horizon = True
			else:
				self.__horizon = False
		# print(p)
		return l, r

	def GetHorizon(self):
		return self.__horizon

	def CalDistance(self):
		d = []
		h = []
		dis = 0
		center_x = 0
		if len(self.__line_h) > 5 and self.__horizon:
			for point in self.__line_h:
				d.append(point[1])
			d.remove(max(d))
			d.remove(min(d))
			for i in range(len(d)):
				dis += d[i]
			dis /= len(d)
			x = 1 - dis / self.height
			self.__distance = 80.465 * (x ** 3) - 65.415 * (x ** 2) + 46.215 * x + 11.646

		if len(self.__line) > 5:
			for point in self.__line:
							h.append(point[0])
			h.remove(max(h))
			h.remove(min(h))
			for i in range(len(h)):
				center_x += h[i]
			center_x /= len(h)

			self.cross = (int(center_x), int(dis))

		return self.__distance

	def CalDirection(self):
		scopes = []
		k = 0
		if len(self.__line) == 8:
			for i in range(len(self.__line) - 1):
				scopes.append(Scope(self.__line[i], self.__line[i + 1]))
			scopes.remove(min(scopes))
			scopes.remove(max(scopes))
			for i in scopes:
				k += i
			k /= len(scopes)
		self.__scope = k

		if k > 20:
			self.direction = Right
		elif k < -20:
			self.direction = Left
		else:
			self.direction = Straight

	def Track(self, frame):
		self.__line.clear()
		self.__line_h.clear()
		self.cross = (0, 0)
		rois = []

		thresh = self.Processing(frame)
		self.FindLine()
		l, r = self.HasHorizon()
		self.FindHosLine(l, r)
		self.CalDirection()
		dis = self.CalDistance()

		if len(self.__line) or len(self.__line_h):
			for point in self.__line:
				cv.circle(self.__frame, point, 3, (255, 0, 0), -1)
			for point in self.__line_h:
				cv.circle(self.__frame, point, 3, (0, 255, 0), -1)
		else:
			# print(len(self.__line), len(self.__line_h))
			pass

		if self.cross != (0, 0):
			cv.circle(self.__frame, self.cross, 5, (125, 125, 125), -1)
			cv.putText(self.__frame, "Distance : {}".format(self.__distance), (0, 20), cv.FONT_HERSHEY_SIMPLEX, 0.6,
			           (0, 255, 0))
			cv.imshow("Circle", self.__frame)
			if 35 <= dis <= 36:
				cv.waitKey(0)
				x, y = self.cross
				rois = [(x - XX, x - X, y + Y_UP, y + Y_DOWN), (x + X, x + XX, y + Y_UP, y + Y_DOWN),
				        (x - 2 * XX, x - X - XX, y + Y_UP, y + Y_DOWN), (x + X + XX, x + 2 * XX, y + Y_UP, y + Y_DOWN)]
				cv.rectangle(self.__frame, (rois[0][0], rois[0][2]), (rois[0][1], rois[0][3]), (0, 255, 0), 2)
				cv.rectangle(self.__frame, (rois[1][0], rois[1][2]), (rois[1][1], rois[1][3]), (0, 255, 0), 2)
				cv.rectangle(self.__frame, (rois[2][0], rois[2][2]), (rois[2][1], rois[2][3]), (0, 255, 0), 2)
				cv.rectangle(self.__frame, (rois[3][0], rois[3][2]), (rois[3][1], rois[3][3]), (0, 255, 0), 2)

				# print(rois)
				cv.waitKey(0)
				return dis, [self.__frame[rois[0][0]:rois[0][1], rois[0][2]:rois[0][3]],
				             self.__frame[rois[1][0]:rois[1][1], rois[1][2]:rois[1][3]],
				             self.__frame[rois[2][0]:rois[2][1], rois[2][2]:rois[2][3]],
				             self.__frame[rois[3][0]:rois[3][1], rois[3][2]:rois[3][3]]]
			else:
				return dis, rois
			# return dis, [self.__frame[rois[0][2]:rois[0][3], rois[0][0]:rois[0][1]], self.__frame[rois[1][2]:rois[1][3], rois[1][0]:rois[1][1]],
			#              self.__frame[rois[2][2]:rois[2][3], rois[2][0]:rois[2][1]], self.__frame[rois[3][2]:rois[3][3], rois[3][0]:rois[3][1]]]


			# cv.imshow("s", self.__frame[rois[0][0]:rois[0][1], rois[0][2]:rois[0][3]])
